#ifndef PROPERTIES
#define PROPERTIES

#include <iostream>
#include <ros/ros.h>
#include <string>
#include <geometry_msgs/Pose.h>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <map>
#include <boost/thread.hpp>
#include <boost/bind.hpp>

using namespace cv;
using namespace std;
/**
 * @brief AGV小车的常属性结构体
 */
struct AgvConstInfo
{
    string RobotSeriesNum_;             ///<小车序列号
    int RobotId_;                       ///<AGV编号
    float Length_;                      ///<小车长度
    float Wide_;                        ///<小车宽度
    float Height_;                      ///<小车高度
    float Payload_;                     ///<最大承重
    float LineSpeedMax_;              	///<最大线速度
    float RotateSpeedMax_;            	///<最大角速度
    float LineAccelerateMax_;           ///<最大线加速度
    float LineSpeedMin_;                ///<最小线速度
    float RotateSpeedMin_;            	///<最小角速度
    float LineAccelerateMin_;           ///<最小线加速度
};
struct CargoInfo{
    string CargoSpecies_;               ///<货物的品种
    bool EmptyOrFilled;                 ///<状态是空还是满
    bool IfJudgeSpecies_;               ///<是否需要判断品种
};
enum AgvStatues{
    IDLE,                   ///<空闲
    WORKING,                ///<工作
    CHARGING,               ///<充电
    ERROR,                  ///<报错
    OFFLINE                 ///<掉线
};

enum MissionType{
    TRANSPORT,                              ///<搬运任务
    CUSTOM                                  ///<自定义任务
};
enum MissionStatue{
    WAITING,
    EXECUTE,
    FINISH
};
/**
 * @brief 任务的属性结构体
 */
struct  MissionInfo
{
    MissionType MissionType_;               ///<任务类型
    string MissionName_;                    ///<任务名字
    int mission_id_;                        ///<任务编号
    string MissionSection_;                 ///<任务所属工序段
    CargoInfo CargoInfo_;                   ///<物品的信息
    int Priority_;                          ///<优先级
    geometry_msgs::Pose StartPose_;         ///<起始点位置
    geometry_msgs::Pose EndPose_;           ///<目标点位置
};

enum TaskType{
    NAVIGATION,
    EXECUTEACTION
};
enum ActionType{
    GET=2,
    PUT
};
struct WorkArea
{
    cv::Point2f LeftDownPoint_;         ///<坐下角点坐标
    float area_length_;                 ///<区域长度
    float area_width_;                  ///<区域宽度
};
/**
 * @brief AGV小车的属性类
 * GetProductionLine 获取Agv小车产线
 * SetProductionLine 设置Agv小车产线
 * GetProcessSection 获取Agv小车工序段
 * SetProcessSection 设置Agv小车工序段
 * GetRobotStatues 获取Agv小车状态
 * SetRobotStatues 设置Agv小车状态
 * GetAgvPose 获取Agv小车位姿
 * SetAgvPose 设置Agv小车位姿
 * GetElectricity 获取Agv小车电量
 * SetElectricity 设置Agv小车电量
 * GetAgvConstInfo 获取Agv小车常属性
 */
class Agv{
    public:
        Agv(string processsection,AgvStatues robotstatues,
            geometry_msgs::Pose globalpose,float electricity,AgvConstInfo agvconstinfo){
            ProcessSection_ = processsection;
            RobotStatues_ = robotstatues;
            GlobalPose_ = globalpose;
            Electricity_ =  electricity;
            AgvConstInfo_ = agvconstinfo;
            InStorePlace_ = false;
        }
        string GetProcessSection(){
            return ProcessSection_;
        }
        void SetProcessSection(string process_section){
            ProcessSection_ = process_section;
        }
        AgvStatues GetRobotStatues(){
            return RobotStatues_;
        }
        void SetRobotStatues(AgvStatues robot_statues){
            RobotStatues_ = robot_statues;
        }
        geometry_msgs::Pose GetAgvPose(){
            return GlobalPose_;
        }
        void SetAgvPose(geometry_msgs::Pose pose){
            GlobalPose_ = pose;
        }
        float GetElectricity(){
            return Electricity_;
        }
        void SetElectricity(float electricity){
            Electricity_ = electricity;
        }
        AgvConstInfo GetAgvConstInfo(){
            return AgvConstInfo_;
        }
        void SetInStorePlace(bool in_store_place){
            InStorePlace_ = in_store_place;
        }
        bool GetInStorePlace(){
            return InStorePlace_;
        }
    private:
        AgvConstInfo AgvConstInfo_;             ///<常属性
        string ProcessSection_;                 ///<所属工序段
        AgvStatues RobotStatues_;               ///<工作状态
        geometry_msgs::Pose GlobalPose_;        ///<全局位姿
        float Electricity_;                     ///<当前电量
        bool InStorePlace_;                     ///<是否在存储点
};

/**
 * @brief 用于管理Agv小车信息的类
 * AddAgv 将Agv对象添加到容器中
 * DeleteAgv 将某个Agv对象从容器中删除
 * ClearAgv 清空Agv小车容器
 * AgvInfoUpdate 更新agv位置和电量信息
 * AgvSelect 将等待的任务分配给Agv
 * AgvsSize 小车数量
 */
class AgvManager{
    public:
        void AddAgv(Agv agv){
            agvs_.insert(pair<int,Agv>(agv.GetAgvConstInfo().RobotId_,agv));
        }
        void DeleteAgv(int id){
            map<int,Agv>::iterator key = agvs_.find(id);
            if(key!=agvs_.end())
            {
              agvs_.erase(key);
            }
        }
        void ClearAgv(){
            agvs_.clear();
        }
        void AgvInfoUpdate(int agv_id,geometry_msgs::Pose GlobalPose,float Electricity){
            agvs_.at(agv_id).SetAgvPose(GlobalPose);
            agvs_.at(agv_id).SetElectricity(Electricity);
        }
        int AgvSelect(MissionInfo mission_info){
            //将等待的任务分配给Agv
            //1.判断有无空闲Agv
            //2.根据工序段判断是否满足要求
            //3.判断是否有品种和空满中状态要求
            //4.选出距离最近的小车
            map<int,Agv>::iterator it;
            map<int,float> distances;
            vector<float> distances_sec;
            for(it=agvs_.begin();it!=agvs_.end();it++){
                if(it->second.GetRobotStatues() == IDLE){
                    if(it->second.GetProcessSection() == mission_info.MissionSection_){
                        if(mission_info.CargoInfo_.EmptyOrFilled == 1){
                            if(mission_info.CargoInfo_.IfJudgeSpecies_ == true){
                                //根据品种进行分类CargoSpecies_
                            }
                            else{
                                float dis=sqrt((mission_info.EndPose_.position.x-it->second.GetAgvPose().position.x)*(mission_info.EndPose_.position.x-it->second.GetAgvPose().position.x)
                                              +(mission_info.EndPose_.position.y-it->second.GetAgvPose().position.y)*(mission_info.EndPose_.position.y-it->second.GetAgvPose().position.y));
                                distances[it->first] = dis;
                                distances_sec.push_back(dis);
                            }
                        }
                        else{
                            float dis=sqrt((mission_info.EndPose_.position.x-it->second.GetAgvPose().position.x)*(mission_info.EndPose_.position.x-it->second.GetAgvPose().position.x)
                                          +(mission_info.EndPose_.position.y-it->second.GetAgvPose().position.y)*(mission_info.EndPose_.position.y-it->second.GetAgvPose().position.y));
                            distances[it->first] = dis;
                            distances_sec.push_back(dis);
                        }
                    }
                }
            }
            if(distances_sec.size()!=0){
                map<int,float>::iterator it_d;
                std::sort(distances_sec.begin(),distances_sec.end());
                for(it_d=distances.begin();it_d!=distances.end();it_d++){
                    if(it_d->second == distances_sec[0]){
                        return it_d->first;
                    }
                }
            }
            else{
                return -1;
            }
        }
        int AgvsNum(){
            return agvs_.size();
        }
        map<int,Agv> agvs_;
};

/**
 * @brief 子任务的属性类
 * Task 构造函数初始化子任务类型
 * DistributionTask 分发任务（虚函数）
 * GetTaskType 获取子任务类型
 * GetTaskStatue 获取子任务状态
 * SetTaskStatue 设置子任务状态
 */
class Task{
    public:
        Task(TaskType task_type):TaskType_(task_type),TaskStatue_(MissionStatue::WAITING){;}
        virtual void DistributionTask(){
        }
        virtual geometry_msgs::Pose GetEndPose(){
            return pose_;
        }
        virtual ActionType GetActionType(){
            return Type_;
        }
        TaskType GetTaskType(){
            return TaskType_;
        }
        MissionStatue GetTaskStatue(){
            return TaskStatue_;
        }
        void SetTaskStatue(MissionStatue task_statue){
            TaskStatue_ = task_statue;
        }
    private:
        TaskType TaskType_;                    ///<子任务类型
        MissionStatue TaskStatue_;             ///<子任务状态
        geometry_msgs::Pose pose_;
        ActionType Type_;
};

/**
 * @brief 导航任务类
 * NavigationTask 构造函数
 * DistributionTask 分发任务
 */
class NavigationTask:public Task{
    public:
        NavigationTask(geometry_msgs::Pose end_pose,string section,TaskType task_type):Task(task_type){
            TaskEndPose_ = end_pose;
            Section_ = section;
        }
        void DistributionTask(){

        }
        geometry_msgs::Pose GetEndPose(){
            return TaskEndPose_;
        }
    private:
        geometry_msgs::Pose TaskEndPose_;                ///<目标点位置
        string Section_;                                 ///<所属工序段
};

/**
 * @brief 执行动作任务类
 * ExecuteActionTask 构造函数
 * DistributionTask 分发任务
 */
class ExecuteActionTask:public Task{
    public:
        ExecuteActionTask(MissionInfo mission_info,ActionType action_type,TaskType task_type):Task(task_type){
            MissionInfo_ = mission_info;
            ActionType_ = action_type;
        }
        void DistributionTask(){}
        ActionType GetActionType(){
            return ActionType_;
        }
    private:
        MissionInfo MissionInfo_;           ///<货物信息
        ActionType ActionType_;             ///<动作类型
};

/**
 * @brief 任务的属性类
 * Mission 构造函数初始化部分成员变量
 * GetMissionInfo() 获取任务信息
 * GetMissionStatue 获取任务状态
 * SetMissionStatue 设置任务状态
 * AddTask() 添加子任务
 * GetTasks() 获取子任务集合
 * GetCurrentTask() 获取当前任务
 * SwitchCurrentTaskStatue() 更改当前任务状态
 */
class Mission{
    public:
        Mission(MissionInfo mission_info,MissionStatue mission_statue){
            MissionInfo_ = mission_info;
            MissionStatue_ = mission_statue;
        }
        MissionInfo GetMissionInfo(){
            return MissionInfo_;
        }
        MissionStatue GetMissionStatue(){
            return MissionStatue_;
        }
        void SetMissionStatue(MissionStatue mission_statue){
            MissionStatue_ = mission_statue;
        }
        void AddTask(std::shared_ptr<Task> task){
            tasks_.push_back(task);
        }
        void AutoBuildTask(){
            tasks_.clear();
            if(MissionInfo_.MissionType_ == MissionType::TRANSPORT){
                shared_ptr<Task> task1(new NavigationTask(MissionInfo_.StartPose_,MissionInfo_.MissionSection_,TaskType::NAVIGATION));
                tasks_.push_back(task1);
                shared_ptr<Task> task2(new ExecuteActionTask(MissionInfo_,ActionType::GET,TaskType::EXECUTEACTION));
                tasks_.push_back(task2);
                shared_ptr<Task> task3(new NavigationTask(MissionInfo_.EndPose_,MissionInfo_.MissionSection_,TaskType::NAVIGATION));
                tasks_.push_back(task3);
                shared_ptr<Task> task4(new ExecuteActionTask(MissionInfo_,ActionType::PUT,TaskType::EXECUTEACTION));
                tasks_.push_back(task4);
            }
            else if(MissionInfo_.MissionType_ == MissionType::CUSTOM){

            }
        }
//        Task& GetNextTasks(){
//            for(int i=0;i<tasks_.size();i++){
//                if(tasks_[i].GetTaskStatue() == MissionStatue::WAITING){
//                    return tasks_[i];
//                }
//            }
//        }
        int GetCurrentTask(){
            for(int i=0;i<tasks_.size();i++){
                if(tasks_[i]->GetTaskStatue() == MissionStatue::WAITING){
                    return i;
                }
            }
            return -1;
        }
//        void SwitchCurrentTaskStatue(MissionStatue statue){
//            GetCurrentTask().SetTaskStatue(statue);
//        }
        vector<shared_ptr<Task>> tasks_;
    private:
        MissionInfo MissionInfo_;               ///<任务信息
        MissionStatue MissionStatue_;           ///<任务状态(等待/正在执行/完成)
};

/**
 * @brief 用于管理任务的类
 * AddMission 添加任务
 * DeleteMission 删除已经完成的任务
 * GetFirstMissions 优先级最高的等待
 */
class MissionManager{
    public:
        void AddMission(Mission mission){
            missions_.insert(make_pair(mission.GetMissionInfo().Priority_,mission));
        }
        void DeleteMission(){
            multimap<int,Mission>::iterator it;
            for(it = missions_.begin();it!=missions_.end();it++){
                if((*it).second.GetMissionStatue() == MissionStatue::FINISH){
                    missions_.erase(it);
                }
            }
        }
        Mission GetFirstMissions(){
            multimap<int,Mission>::iterator iter;
            for(iter = missions_.begin();iter!=missions_.end();iter++){
                if(iter->second.GetMissionStatue() == MissionStatue::WAITING);
                return iter->second;
            }
        }
        multimap<int,Mission> missions_;
};

/**
 * @brief 定义了基础任务点信息
 * PointBase 构造函数初始
 * GetPose 获取定位点位姿
 * SetKeyPose 设置点位姿
 * Getname 获取名字
 * SetName 设置名字
 * SetAvailable 设置状态
 * GetAvailable 获取状态
 */
class PointBase
{
    public:
        PointBase(string name, geometry_msgs::Pose pose,bool available):name_(name),pose_(pose),available_(available){;}
        geometry_msgs::Pose GetKeyPose(){
            return pose_;
        }
        void SetKeyPose(geometry_msgs::Pose pose){
            pose_ = pose;
        }
        string Getname(){
            return name_;
        }
        void SetName(string name){
            name_ = name;
        }
        void SetAvailable(bool available){
            available_ = available;
        }
        bool GetAvailable(){
            return available_;
        }
    private:
        string name_;                    ///<工位名称
        geometry_msgs::Pose pose_;       ///<定位点坐标
        bool available_;                 ///<是否为可用状态
};

/**
 * @brief 设置工作点相关信息
 * WorkPlaceArea 构造函数初始化子任务类型
 * GetWorkArea 获取工作区域
 * SetWorkArea 设置工作区域
 */
class WorkPlaceArea:public PointBase
{
    public:
        WorkPlaceArea(string name,geometry_msgs::Pose pose,bool available,WorkArea area):
            WorkArea_(area),PointBase(name,pose,available){;}
        WorkArea GetWorkArea(){
            return(WorkArea_);
        }
        void SetWorkArea(WorkArea work_area){
            WorkArea_ = work_area;
        }
    private:
        WorkArea WorkArea_;             ///工作区域
};

/**
 * @brief 获取充电桩相关信息
 * GetChargerId 获取充电桩编号
 * GetChargerLength 获取充电桩位置所占长度
 * GetChargerWidth 获取充电桩位置所占宽度
 */
class ChargerPoint:public PointBase
{
    public:
        ChargerPoint(int id,float charger_length,float charger_width,string name,geometry_msgs::Pose pose,bool charger_available):
        ChargerId_(id),Charger_length_(charger_length),Charger_width_(charger_width),PointBase(name,pose,charger_available){;}
        int GetChargerId(){
            return(ChargerId_);
        }
        float GetChargerLength(){
            return Charger_length_;
        }
        float GetChargerWidth(){
            return Charger_width_;
        }
    private:
        int ChargerId_;                  ///<充电桩编号
        float Charger_length_;           ///<充电桩位置所占长度
        float Charger_width_;            ///<充电桩位置所占长度
};

/**
 * @brief 充电桩管理类
 * AddCharger 添加充电桩
 * SelectFreeCharger 选择空闲充电桩
 */
class ChargerManager
{
    public:
        void AddCharger(ChargerPoint charger){
            Chargers_.push_back(charger);
        }
        int SelectFreeCharger(geometry_msgs::Pose pose){
            //在可用的中选择一个最近的充电桩，返回充电桩编号
            float dis_mid = 100000;
            int charger_id = -1;
            for(int i=0;i<Chargers_.size();i++){
                if(Chargers_[i].GetAvailable()==true){
                    float dis=(Chargers_[i].GetKeyPose().position.x-pose.position.x)*(Chargers_[i].GetKeyPose().position.x-pose.position.x)
                    +(Chargers_[i].GetKeyPose().position.y-pose.position.y)*(Chargers_[i].GetKeyPose().position.y-pose.position.y);
                    if(dis<dis_mid){
                        dis_mid = dis;
                        charger_id = Chargers_[i].GetChargerId();
                    }
                }
            }
            return charger_id;
        }
        vector<ChargerPoint> Chargers_;
};

/**
 * @brief 获取存储点相关信息
 * GetStoreId 获取存储点编号
 * GetStoreLength 获取存储点位置所占长度
 * GetStoreWidth 获取存储点位置所占宽度
 */
class StorePlace:public PointBase{
    public:
    StorePlace(int id,float store_length,float store_width,string name,geometry_msgs::Pose pose,bool store_available):
    StoreId_(id),Store_length_(store_length),Store_width_(store_width),PointBase(name,pose,store_available){;}
        int GetStoreId(){
            return(StoreId_);
        }
        float GetStoreLength(){
            return Store_length_;
        }
        float GetStoreWidth(){
            return Store_width_;
        }
    private:
        int StoreId_;                  ///<存储点编号
        float Store_length_;           ///<存储点位置所占长度
        float Store_width_;            ///<存储点位置所占长度
};

/**
 * @brief 存储点管理类
 * AddCharger 添加存储点
 * SelectFreeCharger 选择空闲存储点
 */
class StorePlaceManger{
    public:
        void AddStore(StorePlace storeplace){
            StorePlaces_.push_back(storeplace);
        }
        int SelectFreeStore(geometry_msgs::Pose pose){
            //在可用的中选择一个最近的存储点，返回存储点编号
            float dis_mid = 10000;
            int store_id = -1;

            for(int i=0;i<StorePlaces_.size();i++){
              ROS_INFO("StorePlaces_[i].GetKeyPose().position.x:::%f",StorePlaces_[i].GetKeyPose().position.x);
              ROS_INFO("pose.position.x:::%f",pose.position.x);
                if(StorePlaces_[i].GetAvailable()==true){
                    float dis=(StorePlaces_[i].GetKeyPose().position.x-pose.position.x)*(StorePlaces_[i].GetKeyPose().position.x-pose.position.x)
                    +(StorePlaces_[i].GetKeyPose().position.y-pose.position.y)*(StorePlaces_[i].GetKeyPose().position.y-pose.position.y);
                    ROS_INFO("dis.SIZE():::%f",dis);
                    if(dis<dis_mid){
                        dis_mid = dis;
                        store_id = StorePlaces_[i].GetStoreId();
                    }
                }
            }
            return store_id;
        }
        vector<StorePlace> StorePlaces_;
};
class WayPoint:public PointBase
{
    public:
        WayPoint(int id,string name,geometry_msgs::Pose pose,bool available):
        WayPointId_(id),PointBase(name,pose,available){;}
    private:
        int WayPointId_;                ///<路径点编号
};

/**
 * @brief 路段的相关属性
 * Route 构造函数用于初始化成员变量
 * GetLength 获取路径长度
 * GetPassType 获取路径类型
 * GetBundling 获取路段独占情况
 */
class Route{
    public:
        Route(int length,bool pass_type,bool bundling):
            Length_(length),PassType_(pass_type),Bundling_(bundling){;}
        float GetLength(){
            return Length_;
        }
        bool GetPassType(){
            return PassType_;
        }
        bool GetBundling(){
            return Bundling_;
        }
    private:
        float Length_;                      //长度；
        bool PassType_;                     //通行类型（单向通行/双向通行）；
        bool Bundling_;                     //判断是否是独占路径；
        vector<cv::Point2f> WayPoint_;      //路径上包含的点坐标
        pair<int,int> StartToEnd_;          //起始点和终止点编号
        float Busyness_;                    //繁忙程度
};
/**
 * @brief 路径点的相关属性
 */
class PathNode{
    int NodeId;                 //节点编号
    cv::Point2f NodePoint;      //节点坐标
};
/**
 * @brief 路段和路径点的管理类
 * AddRoute 添加一段路段
 * DeleteSingleRoute 删除一段路段
 * DeletePeriodRoute 删除连续路段
 * ClearRoute 清空路段
 * AddNode 添加一个路径点
 * DeleteSingleNode 删除一个路径点
 * DeletePeriodNode 删除连续路径点
 * ClearNode 清空路径点
 */
class RoutesNodesManger{
    public:
        void AddRoute(){
            Routes_.push_back(Route_);
        }
        void DeleteSingleRoute(int i){
            Routes_.erase(Routes_.begin()+i);
        }
        void DeletePeriodRoute(int i,int j){
            Routes_.erase(Routes_.begin()+i,Routes_.begin()+j);
        }
        void ClearRoute(){
            Routes_.clear();
        }
        void AddNode(){
            Nodes_.push_back(Node_);
        }
        void DeleteSingleNode(int i){
            Nodes_.erase(Nodes_.begin()+i);
        }
        void DeletePeriodNode(int i,int j){
            Nodes_.erase(Nodes_.begin()+i, Nodes_.begin()+j);
        }
        void ClearNode(){
            Nodes_.clear();
        }
    private:
        Route Route_;
        vector<Route> Routes_;
        PathNode Node_;
        vector<PathNode> Nodes_;
};

#endif
